#!/bin/bash

# 重建工作空间
echo "Rebuilding workspace ..."
unset AMENT_PREFIX_PATH
unset CMAKE_PREFIX_PATH
rm -rf install/ build/ log/
source /opt/ros/humble/setup.bash
colcon build --symlink-install
source install/setup.bash

echo "................................."

# 命令菜单定义
commands=(
    "ros2 launch zorbus_description display.launch.py"
    "ros2 launch zorbus_description control.launch.py"
    "ros2 launch zorbus_description gazebo.launch.py"
    "ros2 launch moveit_setup_assistant setup_assistant.launch.py"
    "ros2 launch zorbus_moveit_config demo.launch.py"
    "ros2 run zorbus_image zorbus_image_publisher.py"
    "ros2 run zorbus_image zorbus_image_subscriber.py"
    "退出菜单"
)

# 显示命令菜单
show_menu() {
    clear
    echo "============== 命令菜单 =============="
    for i in "${!commands[@]}"; do
        printf "%2d) %s\n" $((i+1)) "${commands[$i]}"
    done
    echo "====================================="
    read -p "请选择要执行的命令 (输入序号): " choice
    handle_choice $choice
}

# 处理用户选择
handle_choice() {
    local selected=$1

    # 验证输入
    if ! [[ "$selected" =~ ^[0-9]+$ ]]; then
        echo -e "\n\e[31m错误：请输入有效数字\e[0m"
        sleep 1
        return
    fi

    # 检查范围
    if ((selected < 1 || selected > ${#commands[@]})); then
        echo -e "\n\e[31m错误：序号超出范围 (1-${#commands[@]})\e[0m"
        sleep 1
        return
    fi

    # 退出处理
    if ((selected == ${#commands[@]})); then
        echo "退出菜单..."
        exit 0
    fi

    # 执行命令
    local cmd_index=$((selected-1))
    echo -e "\n\e[32m执行命令: ${commands[$cmd_index]}\e[0m"
    echo "-------------------------------------"
    eval "${commands[$cmd_index]}"

    # 等待用户确认后返回菜单
    echo "-------------------------------------"
    read -n 1 -s -p "命令执行完成，按任意键返回菜单..."
}

# 主循环
while true; do
    show_menu
done
